/** Interactive Surface  FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *
 *  @author Acer
 */
package test.shashindra;

import com.googlecode.javacv.cpp.opencv_video;
import com.googlecode.javacv.cpp.opencv_video.*;
import com.googlecode.javacv.cpp.opencv_video.CvKalman;
import com.googlecode.javacv.cpp.opencv_video.*;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_core.CvMat;

/** Interactive Surface FYP- 25 Interactive Surface FYP- 25 Interactive Surface FYP- 25
 *  General Information about this class -
 *  Comments -
 */
public class Kalman {

    CvKalman filter1 = CvKalman.create(4, 2, 0);
    //cvCreateKalman(4, 2, 0);

    {

        CvMat gh = CvMat.create(4, 4);
        gh.put(0, 0, 1);
        gh.put(0, 1, 0);
        gh.put(0, 2, 1);
        gh.put(0, 3, 0);

        gh.put(1, 0, 0);
        gh.put(1, 1, 1);
        gh.put(1, 2, 0);
        gh.put(1, 3, 1);

        gh.put(0, 0, 1);
        gh.put(0, 1, 0);
        gh.put(0, 2, 1);
        gh.put(0, 3, 0);

        gh.put(2, 0, 0);
        gh.put(2, 1, 0);
        gh.put(2, 2, 1);
        gh.put(2, 3, 0);

        gh.put(1, 0, 0);
        gh.put(1, 1, 1);
        gh.put(1, 2, 0);
        gh.put(1, 3, 1);

        gh.put(2, 0, 0);
        gh.put(2, 1, 0);
        gh.put(2, 2, 1);
        gh.put(2, 3, 0);

        gh.put(3, 0, 0);
        gh.put(3, 1, 0);
        gh.put(3, 2, 0);
        gh.put(3, 3, 1);

        filter1.transition_matrix(gh);
        CvMat measurement = CvMat.create(2, 1);

        CvMat stateMat = CvMat.create(2, 2);
        stateMat.put(0, 0, 0);
        stateMat.put(1, 0, 0);
        stateMat.put(1, 1, 0);
        stateMat.put(0, 1, 0);

        filter1.state_pre(stateMat);
        filter1.MeasurementMatr(null);
        filter1.process_noise_cov(CvMat.create(1, 1).put(0, 0, 1e-4));
        filter1.measurement_noise_cov(CvMat.create(1, 1).put(0, 0, 1e-1));
        filter1.error_cov_post(CvMat.create(1, 1).put(0, 0, 0.1));
       

    }

//KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
//Mat_<float> measurement(2,1); measurement.setTo(Scalar(0));
//
//// Get mouse point
//measurement(0) = mouse_info.x;
//measurement(1) = mouse_info.y;
//
//Point measPt(measurement(0),measurement(1));
//
//// The "correct" phase that is going to use the predicted value and our measurement
//Mat estimated = KF.correct(measurement);
//Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    public static void main(String[] args) {
        Kalman km = new Kalman();
        int[] yt=new int[2];
        double[] res;
        for(int y=0;y<20;y++){
            yt[0]=y+1;
            yt[1]=2*y+1;
            res=km.getState();
            System.out.println(res[0]+","+res[1]);
            km.update(yt);
            res=km.getState();
            System.out.println(res[0]+","+res[1]);
        }

    }

    double[] getState() {
        double[] res = new double[2];
//        CvMat prediction = opencv_video.cvKalmanPredict(filter1, null);
//        res[0]=prediction.get(0);
//        res[1]=prediction.get(1);
         CvMat prediction =filter1.state_post();
        res[0]=prediction.get(0);
        res[1]=prediction.get(1);
         return res;
    }

    void update(int[] res){

        CvPoint predictPt = new CvPoint();
        predictPt.put(res[0], res[1]);

        CvMat stateMat = CvMat.create(1, 4);
        stateMat.put(0, 0, res[0]);
        stateMat.put(0, 1, res[1]);
        
        opencv_video.cvKalmanPredict(filter1, stateMat);


    }
}
